#include "stm32f10x.h"                  // Device header
#include "Timer.h"
#include "Delay.h"
#include "OLED.h"
#include "Serial.h"
#include "MPU6050.h"
#include "ultrasonic.h"
#include "JDY68A.h"
#include "BalanceCar.h"
#include "BalanceCar_Set.h"

//volatile uint8_t MPU6050Status = 0;		// 陀螺仪采样标志位

uint8_t status1 = 1;
uint32_t pulse_duration;
IMU_Attitude att;

//uint16_t Error = 0;// 错误打印

void PIDTask(void)
{
	static int8_t OuterStatus = 0;// 外环调控标志位
	
//	OuterStatus++;
			
	uint32_t current_count, pulse_start_time;

	current_count = TIM_GetCounter(TIM3);
	pulse_start_time = current_count;
	
	IMU_Update();
	// 获取姿态数据用于控制
	att = IMU_GetAttitude();
	
	current_count = TIM_GetCounter(TIM3);
	
	uint32_t end_time = current_count;
	pulse_duration = end_time - pulse_start_time;
	
	if(att.roll_deg > -50 && att.roll_deg < 50 && status1 == 1) 
	{
		BalanceCar_InnerPID(att.roll_deg);
	
		if(OuterStatus >= 6)
		{
			OuterStatus = 0;
			BalanceCar_OuterPID();
		}
	}
	else
	{
		status1 = 0;
		PWM_SetDuty(0, 0);
		PWM_SetDuty(1, 0);
	}
}

int main(void)
{
	OLED_Init();
	IMU_Init();
	Serial_Init();

	JDY68A_Init();
	Ultrasonic_Init();
	BalanceCar_Init();
	
	MPU_Calib();
	
	Timer_Init();
	
	while (1)
	{
		PIDDebug();
		
		Ultrasonic_Process();
		
		// 显示距离
		float distance = Ultrasonic_GetDistance();
		if (distance > 0) 
		{
			OLED_Printf(0, 0, OLED_6X8, "Dist:%05.0f", distance);
		}
		
		OLED_Printf(0, 8, OLED_6X8, "AX:%+05.1f", att.roll_deg);
		OLED_Printf(0, 16, OLED_6X8, "ti:%05d", pulse_duration);
		
//		OLED_Printf(64, 0, OLED_6X8, "lo1:%+05d", Location1);
//		OLED_Printf(64, 8, OLED_6X8, "lo2:%+05d", Location2);

		
		OLED_Printf(64, 0, OLED_6X8, "Tar1:%+05.1f", LeftPID[0].Target);
		OLED_Printf(64, 8, OLED_6X8, "Act1:%+05.1f", LeftPID[0].Actual);
		OLED_Printf(64, 16, OLED_6X8, "Out1:%+05.0f", LeftPID[0].Out);
		
		OLED_Printf(64, 24, OLED_6X8, "Tar2:%+05.1f", RightPID[0].Target);
		OLED_Printf(64, 32, OLED_6X8, "Act2:%+05.1f", RightPID[0].Actual);
		OLED_Printf(64, 40, OLED_6X8, "Out2:%+05.1f", RightPID[0].Out);
		
		OLED_Printf(0, 32, OLED_6X8, "Act2:%+05.1f", RightPID[0].ErrorInt);
		OLED_Printf(0, 40, OLED_6X8, "Out2:%+05.1f", RightPID[0].ErrorInt);
	
//		Serial_Printf("%f, %f, %f\r\n",LeftPID[0].Target, LeftPID[0].Actual, LeftPID[0].Out);

		OLED_Update();

	}
}

void TIM3_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM3, TIM_IT_Update) == SET)
	{
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
		
		BalanceCar_Read();
		
		PIDTask();
		
		Ultrasonic_StartMeasurement();
	}
}
